#include <Arduino.h>
#include <FreeRTOSConfig.h>
#include <ArduinoJson.h>
#include <HardwareSerial.h>
// #include <WiFi.h>

#include "E53_IA1.h"
#include "E53_IS1.h"
#include "sg90.h"
#include "SR04.h"
#include "cs100.h"


// const char* ssid = "iPhone15";
// const char* pwd = "sztubdi1005";

float distance = 0;
// long distance = 0;

int full_threshold = 7;   //超声波测溢满阈值为7cm
int max_height = 10;      //垃圾桶最大容纳高度为10cm
bool full_flag = false;
int status = 0;     //红外感应结果
int classificate_res_int;   //垃圾分类结果对应的垃圾桶序号
Data e53_ia1_data;

uint8_t test_status = 0;

// //声明一个互斥信号量句柄，我们将使用它来管理串口。它将被用来确保在任何时候只有一个任务正在访问这个资源。
// SemaphoreHandle_t xSerialSemaphore;

// TaskHandle_t taskDataDetectHandle = NULL;

// define two Tasks for ClassificationResRead & DataDetectAndSend
void TaskClassificationResRead( void *pvParameters );

void package_data_to_json(Data* temp_and_humi);
void garbage_status_package();

// SR04 sr04 =SR04();

void setup() {
  Serial.begin(115200);
  while(!Serial){};

  
  // // 所有传感器初始化
  E53_IA1_init();
  // Serial.println("E53_IA1 init finish");
  E53_IS1_init();
  // Serial.println("E53_IS1 init finish");
  sg90_init();
  // Serial.println("sg90 init finish");
  // cs100_init();
  // Serial.println("cs100 init finish");


  // /**信号量对于停止一个任务进程是有用的，它应该暂停等待；
  // 因为它正在共享一个资源，比如串口。
  // 信号量应该只在调度程序运行时使用，但是我们可以在这里设置它。*/
  // if ( xSerialSemaphore == NULL )  // Check to confirm that the Serial Semaphore has not already been created.
  // {
  //   xSerialSemaphore = xSemaphoreCreateMutex();  // Create a mutex semaphore we will use to manage the Serial Port
  //   if ( ( xSerialSemaphore ) != NULL )
  //     xSemaphoreGive( ( xSerialSemaphore ) );  // Make the Serial Port available for use, by "Giving" the Semaphore.
  // }

  // // Now set up two Tasks to run independently.
  xTaskCreate(
    TaskClassificationResRead
    ,  "ClassificationResRead"  // A name just for humans
    ,  2048  // This stack size can be checked & adjusted by reading the Stack Highwater
    ,  NULL //Parameters for the task
    ,  2  // Priority, with 3 (configMAX_PRIORITIES - 1) being the highest, and 0 being the lowest.
    ,  NULL ); //Task Handle

  vTaskStartScheduler(); //启动调度
  
  // sg90_change_lid_stat(0, STATE_OPEN);
  // sleep(1);
  // sg90_change_lid_stat(0, STATE_CLOSE);
}

void loop() {

  JsonDocument garbage_data;

  garbage_data["type"] = "health";
  
  JsonObject data = garbage_data.createNestedObject("data");
  data["recyclable_waste"] = 0;
  data["kitchen_waste"] = 0;
  data["hazardous_waste"] = 0;
  data["other_waste"] = 0;


  //检测垃圾桶温度数据
  // E53_IA1_ReadData(&e53_ia1_data);
  // data["temperature"]  = 27.05;
  // data["humidity"]  = 32.64;
  garbage_data["temperature"]  = e53_ia1_data.temp;
  garbage_data["humidity"]  = e53_ia1_data.hum;

  // if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
  // {
    serializeJson(garbage_data, Serial);
    Serial.println();
  //   xSemaphoreGive( xSerialSemaphore );
  // }

  // 检测垃圾到顶部的距离并打印
  // distance = calculate_distance();
  // distance = sr04.Distance();
  // Serial.print(distance);
  // Serial.println("cm");


  //如果垃圾桶溢满，则置溢满标志位为真，并关闭垃圾桶盖子，否则置溢满标志位为假
  // if(distance >= max_height)
  // {
  //   full_flag = true;
  //   // sg90_change_lid_stat(classificate_res_int, STATE_CLOSE);
  // }
  // else{
  //   full_flag = false;
  // }

  // garbage_data["distance"]  = distance;

    //检测红外感应结果，如果感应到垃圾就发送数据到PC串口
    
    if(E53_IS1_Read_Data() == 1)
    {
      // if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
      // {
        Serial.println(1);
        JsonDocument infrared_res;
        infrared_res["type"] = "income";
        infrared_res["data"]  = JsonArray();
        serializeJson(infrared_res, Serial);
        Serial.println();
      //   xSemaphoreGive( xSerialSemaphore );
      // }
    }
    else{
      // if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
      // {
        Serial.println(0);
        sg90_change_lid_stat(classificate_res_int, STATE_CLOSE);   //当检测到没人，就关闭盖子
        // xSemaphoreGive( xSerialSemaphore );
      // }
    }

  delay(3000);    //1s检测一次

}


void TaskClassificationResRead( void *pvParameters )
{
  __unused(pvParameters);
  
  Serial.println("come in TaskClassificationResRead\r\n");
  while (1)
  {
    // if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
    // {
      String classificate_res = "";
      if(Serial.available())
      {
        classificate_res = Serial.readStringUntil('\n');
        // 查找末尾的 '\r' 和 '\n'，并去除它们
        classificate_res.trim();  // 去除字符串前后多余的空格和换行符
        Serial.printf("recv:%s\r\n",classificate_res);

        long classificate_res_int = strtol(classificate_res.c_str(), NULL, 10);
        if (classificate_res_int == 0 && classificate_res != "0") {
            Serial.println("Invalid classification result");
            // xSemaphoreGive( xSerialSemaphore );
            delay(1000);
            continue;
        }
        if((classificate_res_int >= 0) && (classificate_res_int <= 3))
        {
          //  if(!full_flag) {    //获取分类结果对应的盖子，当溢满状态标志位为false时，允许打开对应的盖子
          
            int ret = sg90_change_lid_stat( classificate_res_int, STATE_OPEN);
            if(ret != 0)
            {
              // if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
              // {
                // SerialPC.println("classificated result invalid!");
                Serial.println("classificated result invalid!");
                // xSemaphoreGive( xSerialSemaphore );
              //   xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
              // }
            // }
      }
    }
      // else 
      // {
      //   classificate_res = "4";
      // }
    //   xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
    // }
    // classificate_res_int = atoi(classificate_res.c_str());
    // long classificate_res_int = strtol(classificate_res.c_str(), NULL, 10);
    // if (classificate_res_int == 0 && classificate_res != "0") {
    //     Serial.println("Invalid classification result");
    //     delay(1000);
    //     continue;
    // }
    // if((classificate_res_int >= 0) && (classificate_res_int <= 3))
    // {
    //   //  if(!full_flag) {    //获取分类结果对应的盖子，当溢满状态标志位为false时，允许打开对应的盖子
      
    //     int ret = sg90_change_lid_stat( classificate_res_int, STATE_OPEN);
    //     if(ret != 0)
    //     {
    //       // if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
    //       // {
    //         // SerialPC.println("classificated result invalid!");
    //         Serial.println("classificated result invalid!");
    //       //   xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
    //       // }
    //     }
      // }
      // else    //若溢满标志位为真，禁止打开盖子,打印告警
      // {
      //   if ( xSemaphoreTake( xSerialSemaphore, ( TickType_t ) 5 ) == pdTRUE )
      //     {
      //       // SerialPC.println("garbage is full, open lid failed!");
      //       Serial.println("garbage is full, open lid failed!");
      //       xSemaphoreGive( xSerialSemaphore ); // Now free or "Give" the Serial Port for others.
      //     }
      // }
    }

    delay(1000);  //1s检测一次
  }
}

// {
//     "type": "health",
//     "data": {
//         "recyclable_waste": 0,
//         "kitchen_waste": 0,
//         "hazardous_waste": 0,
//         "other_waste": 0,
//         "temperature": 25.6,
//         "humidity": 30.8
//     }
// }